import math
from plan_tool.Node import Node
from plan_tool.point import point



class Floydnode:
    def __init__(self, point, realpoint, parent=None):
        self.point = point
        self.realpoint = realpoint
        self.parent = parent
        self.distance = 0

    def getDis(self, randNode):  # 距离
        self.distance = math.sqrt(pow(randNode.point[0] - self.point[0], 2) + pow(randNode.point[1] - self.point[1], 2))
        self.distance = self.distance

def Heron(a, b, c):
    p = (a + b + c) / 2
    S = math.sqrt(p * (p - a) * (p - b) * (p - c))
    h = 2 * S / a
    return h


def isSafe(newPoint, nearNode, obstacle):
    nearNode.getDis(Node(newPoint))
    newNear = nearNode.distance
    for i in obstacle:
        newDx = newPoint[0] - i[0]
        newDy = newPoint[1] - i[1]
        nearDx = nearNode.point[0] - i[0]
        nearDy = nearNode.point[1] - i[1]

        newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
        nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))

        h = Heron(newNear, newO, nearO)  # 海伦公式
        cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
        cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear)  # 余弦定理

        if h <= i[2] and cosnew > 0 and cosnear > 0:
            return False
    return True

def isssssSafe(newPoint, nearNode, obstacle):
    nearNode.getDis(Node(newPoint))
    newNear = nearNode.distance
    for i in obstacle:
        newDx = newPoint.x - i[0]
        newDy = newPoint.y - i[1]
        nearDx = nearNode.point.x - i[0]
        nearDy = nearNode.point.y - i[1]

        newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
        nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))

        h = Heron(newNear, newO, nearO)  # 海伦公式
        cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
        cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear)  # 余弦定理

        if h <= i[2] and cosnew > 0 and cosnear > 0:
            return False
    return newNear


def floyd(pathlist, obstacle):
    # 弗洛伊德算法
    obstacle_list = []
    for i in range(len(obstacle)):
        obstacle_list.append([obstacle[i][0],obstacle[i][1],obstacle[i][2] + 3])

    Node_list = []
    for i in range(len(pathlist)): #预处理
        x = pathlist[i][0]
        y = pathlist[i][1]
        true_x = x
        true_y = y
        if i == 0:
            path_node = Floydnode([true_x, true_y], pathlist[i], None)
            Node_list.append(path_node)
        else:
            path_node = Floydnode([true_x, true_y], pathlist[i], Node_list[i - 1])
            Node_list.append(path_node)

    PathList = []
    CurNode = Node_list[len(Node_list) - 1]
    tailNode = CurNode  # 指针

    new_node_list = []
    while CurNode is not None:
        deletflag = 0 # 判断是否可缩
        while deletflag == 0:
            ParentNode = CurNode.parent
            if ParentNode is not None:
                GrandPaNode = ParentNode.parent
            else:
                GrandPaNode = None

            # 消除临近节点
            if ParentNode is not None and GrandPaNode is not None:
                if CurNode.point[0] == ParentNode.point[0] and CurNode.point[0] == GrandPaNode.point[0]:
                    CurNode.parent = GrandPaNode
                elif CurNode.point[1] == ParentNode.point[1] and CurNode.point[1] == GrandPaNode.point[1]:
                    CurNode.parent = GrandPaNode
                elif CurNode.point[1] == ParentNode.point[1] and CurNode.point[1] == ParentNode.point[1]:
                    CurNode.parent = GrandPaNode
                elif round(((CurNode.point[0] - ParentNode.point[0]) / (CurNode.point[1] - ParentNode.point[1] + 0.1), 3) ==
                      round((CurNode.point[0] - GrandPaNode.point[0]) / (CurNode.point[1] - GrandPaNode.point[1] + 0.1)), 3):
                    CurNode.parent = GrandPaNode
                else:
                    deletflag = 1
            else:
                deletflag = 1
        if deletflag == 1:
            new_node_list.append(CurNode)
        CurNode = CurNode.parent

    CurNode = tailNode

    output_list = []
    while CurNode is not None:
        PathList.append(CurNode.point)
        floydflag = 0

        param_b = 0
        param_k = 0
        while floydflag == 0:
            ParentNode = CurNode.parent
            if ParentNode is not None:
                GrandPaNode = ParentNode.parent
            else:
                GrandPaNode = None

            if ParentNode is not None and GrandPaNode is not None:
                distance_AB = math.sqrt(
                    pow(CurNode.point[0] - ParentNode.point[0], 2) + pow(CurNode.point[1] - ParentNode.point[1], 2))
                distance_BC = math.sqrt(
                    pow(ParentNode.point[0] - GrandPaNode.point[0], 2) + pow(ParentNode.point[1] - GrandPaNode.point[1],
                                                                             2))
                distance_AC = math.sqrt(
                    pow(CurNode.point[0] - GrandPaNode.point[0], 2) + pow(CurNode.point[1] - GrandPaNode.point[1], 2))
                total_dis = distance_AB + distance_BC

                if total_dis >= distance_AC:  # 距离可能需修改
                    delta_x = CurNode.point[0] - GrandPaNode.point[0]
                    delta_y = CurNode.point[1] - GrandPaNode.point[1]
                    dist = math.sqrt(pow(delta_x, 2) + pow(delta_y, 2))
                    if abs(delta_x) > abs(delta_y):
                        temp_x = CurNode.point[0]
                        if delta_x > 0:
                            step = - 0.1
                        else:
                            step = 0.1
                        while abs(temp_x - GrandPaNode.point[0]) >= 0.11:
                            param_k = delta_y / delta_x
                            param_b = CurNode.point[1] - param_k * CurNode.point[0]
                            temp_x = temp_x + step
                            temp_y_ceil = math.ceil(param_k * temp_x + param_b)
                            temp_y_floor = math.floor(param_k * temp_x + param_b)
                            isobstacle = 0
                            for i in range(len(obstacle_list)):
                                dis1 = pow(temp_y_ceil - obstacle_list[i][1], 2) + pow(temp_x - obstacle_list[i][0], 2)
                                dis2 = pow(temp_y_floor - obstacle_list[i][1], 2) + pow(temp_x - obstacle_list[i][0], 2)
                                if dis1 <= pow(obstacle_list[i][2], 2) or dis2 <= pow(obstacle_list[i][2], 2):
                                    isobstacle = 1
                                    break
                            if isobstacle == 1:
                                floydflag = 1
                                break

                    elif abs(delta_x) == abs(delta_y):
                        floydflag = 0
                        break
                    else:
                        temp_y = CurNode.point[1]
                        if delta_y > 0:
                            step = - 0.1
                        else:
                            step = 0.1
                        while abs(temp_y - GrandPaNode.point[1]) >= 0.11:
                            param_k = delta_x / delta_y
                            param_b = CurNode.point[0] - param_k * CurNode.point[1]
                            temp_y = temp_y + step
                            temp_x_ceil = math.ceil(param_k * temp_y + param_b)
                            temp_x_floor = math.floor(param_k * temp_y + param_b)
                            isobstacle = 0
                            for i in range(len(obstacle_list)):
                                dis1 = pow(temp_y - obstacle_list[i][1], 2) + pow(temp_x_ceil - obstacle_list[i][0], 2)
                                dis2 = pow(temp_y - obstacle_list[i][1], 2) + pow(temp_x_floor - obstacle_list[i][0], 2)
                                if dis1 <= pow(obstacle_list[i][2], 2) or dis2 <= pow(obstacle_list[i][2], 2):
                                    isobstacle = 1
                                    break
                            if isobstacle == 1:
                                floydflag = 1
                                break
                else:
                    floydflag = 1

                if not isSafe(CurNode.point, GrandPaNode, obstacle_list) and floydflag == 0:
                    floydflag = 1
            else:
                floydflag = 1

            if floydflag == 0:
                CurNode.parent = GrandPaNode
        CurNode = CurNode.parent

    CurNode = tailNode
    while CurNode is not None:
        output_list.append(CurNode.realpoint)
        CurNode = CurNode.parent
    output_list.reverse()

    return output_list

def clean(pathList, obstacle):
    path = []
    tailNode = Node(point(pathList[len(pathList)-1][0],pathList[len(pathList)-1][1]))
    curNode = tailNode
    for i in range(len(pathList)):
        num = len(pathList) - i - 2
        if num >= 0:
            fatherNode = Node(point(pathList[num][0], pathList[num][1]))
            curNode.ancient = fatherNode
            curNode = fatherNode
        else:
            curNode.ancient = None

    curNode = tailNode
    while True:
        if curNode.ancient.ancient is not None:
            # print(curNode.ancient.ancient.point.x)
            diss = isssssSafe(point(curNode.point.x, curNode.point.y), Node(point(curNode.ancient.ancient.point.x, curNode.ancient.ancient.point.y)), obstacle)
            if diss:
                if diss <= 7:
                    curNode.ancient = curNode.ancient.ancient
                else:
                    curNode = curNode.ancient
            else:
                curNode = curNode.ancient
        else:
            break

    curNode = tailNode
    while True:
        if curNode is not None:
            path.append([curNode.point.x,curNode.point.y])
            curNode = curNode.ancient
        else:
            break
    path.reverse()
    return path

def clean_infinite(pathList, obstacle):
    path = []
    tailNode = Node(point(pathList[len(pathList)-1][0],pathList[len(pathList)-1][1]))
    curNode = tailNode
    for i in range(len(pathList)):
        num = len(pathList) - i - 2
        if num >= 0:
            fatherNode = Node(point(pathList[num][0], pathList[num][1]))
            curNode.ancient = fatherNode
            curNode = fatherNode
        else:
            curNode.ancient = None

    curNode = tailNode
    while True:
        if curNode.ancient.ancient is not None:
            # print(curNode.ancient.ancient.point.x)
            diss = isssssSafe(point(curNode.point.x, curNode.point.y), Node(point(curNode.ancient.ancient.point.x, curNode.ancient.ancient.point.y)), obstacle)
            if diss:
                curNode.ancient = curNode.ancient.ancient
            else:
                curNode = curNode.ancient
        else:
            break

    curNode = tailNode
    while True:
        if curNode is not None:
            path.append((curNode.point.x,curNode.point.y))
            curNode = curNode.ancient
        else:
            break
    path.reverse()
    return path